
//LD293
const int moterLogic1a = 7;
const int moterLogic1b = 8;
const int moterLogic1e = 9;
const int moterLogic2a = 4;
const int moterLogic2b = 5;
const int moterLogic2e = 10;
const int speedPoti = A0;

const int maxStopDelay = 500;
unsigned long lastStop = 505;

int speed = 100;
int SollSpd = 0;
int msRampe = 2000;

int Richtung;
const int dir_forward = 0;
const int dir_backward = 1;
const int dir_left = 2;
const int dir_right = 3;

void motorSetup(){
  pinMode(moterLogic1a, OUTPUT);
  pinMode(moterLogic1b, OUTPUT);
  pinMode(moterLogic1e, OUTPUT);
  pinMode(moterLogic2a, OUTPUT);
  pinMode(moterLogic2b, OUTPUT);
  pinMode(moterLogic2e, OUTPUT);
}

void UpdateSpeed(){
  SollSpd = analogRead(speedPoti);
  SollSpd = map(SollSpd, 0, 1023, 120, 255);
  //  Serial.println(SollSpd); Serial.print(';');

  unsigned long TimeDiff;
  TimeDiff = millis() - lastStop;
  //  Serial.print(TimeDiff); Serial.print(';');

  float Faktor;
  Faktor = TimeDiff/float(msRampe);
  if(Faktor > 1.000){
    Faktor = 1.0;
  }
  //  Serial.print(Faktor); Serial.print(';');

  speed = Faktor * SollSpd;
  if(speed < 100){
    speed = 100;
  }
  //  Serial.print(speed); Serial.print(';');

  setSpeed(speed);
}

void resetSpeed(){
  setSpeed(0);
  lastStop=millis();
  UpdateSpeed();
}

void forward(){
  if (Richtung != dir_forward){
    stopp();
  }
  Richtung = dir_forward;
  setMotorLogic(HIGH, LOW, HIGH, LOW);
}

void backward(){
  if (Richtung != dir_backward){
    stopp();
  }
  Richtung = dir_backward;
  setMotorLogic(LOW, HIGH, LOW, HIGH);
}

void right(){
  if (Richtung != dir_right){
    stopp();
  }
  Richtung = dir_right;
  setMotorLogic(HIGH, LOW, LOW, HIGH);
}

void left(){
  if (Richtung != dir_left){
    stopp();
  }
  Richtung = dir_left;
  setMotorLogic(LOW, HIGH, HIGH, LOW);
}

void shortback(int aTime){
  float Faktor;
  Faktor = (1/float(speed))*float(SollSpd);

  aTime = aTime * Faktor;
  if (aTime > 1000){
    aTime=1000;
  }

  stopp();
  backward();

  //Solange Back bis TimeOut oder Sens
  int SENS = sens();
  int tick = millis();
  //while((SENS = sens()) && ((millis()-tick)<aTime)){
  while((SENS = sens()) && ((millis()-tick)<2000)){
    UpdateSpeed();
    delay(5);
  }
  if(SENS != sens()){
    Serial.println("Shortback end: Sens");
  }
  else
  {
    Serial.println("Shortback end: Time");
  }
  delay(5);

  stopp();
}

void stopp() { 
  setSpeed(0);

  int diff=millis()-lastStop;
  if(diff>100){
    float Faktor;
    Faktor = float(speed)/float(SollSpd);
    if(Faktor > 1.000){
      Faktor = 1.0;
    }

    unsigned long time = 0;
    time = maxStopDelay * Faktor;
    if(time < 100){
      time = 100;
    }
    
    Serial.print("Stopp: ");
    Serial.println(time);
    
    delay(time);
  }

  lastStop=millis();
}

void setMotorLogic(byte m11, byte m12, byte m21, byte m22){
  set(moterLogic1a,m11);
  set(moterLogic1b,m12);
  set(moterLogic2a,m21);
  set(moterLogic2b,m22);
}

void setSpeed(int aSpeed) {
  analogWrite(moterLogic1e, aSpeed);
  analogWrite(moterLogic2e, aSpeed);
}


